#!/usr/bin/env python



# Copyright (c) 2011, Stephan Weiss, ASL, ETH Zurich, Switzerland
# You can contact the author at <stephan dot weiss at mavt dot ethz dot ch>
# 
# All rights reserved.
# 
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# * Neither the name of ETHZ-ASL nor the
# names of its contributors may be used to endorse or promote products
# derived from this software without specific prior written permission.
# 
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.


NAME='wpclient'

# ROS specific imports
import roslib; roslib.load_manifest('asctec_hl_interface')
import rospy
import actionlib
import re
import time

import asctec_hl_comm.msg

def parseWaypoints():
    rospy.loginfo("parsing waypoint list...")
    # read file
    text = open("sampleways.txt","r").readlines()
    waypoints = list();

    linecounter = 0
    for line in text:
        linecounter += 1;
        wordcounter = 0
        goodpoint=True;
        if not (line[0]=="#" or not line.strip()):
            for word in line.split():
                wordcounter += 1;
                # check if integer (isdigit) or float
                if not (word.isdigit() or not re.match("^[+-]?\d+?\.\d+?$", word) is None):                
                #if not (word.isdigit() or not re.match("^(0|(-(((0|[1-9]\d*)\.\d+)|([1-9]\d*))))$", word) is None):
                    #print "corrupt waypoint: ", line
                    rospy.logerr("corrupt waypoint: " + line)
                    goodpoint=False
                    break
            if wordcounter<11 and goodpoint:
                #print "warning: only ", wordcounter, " arguments provided"
                rospy.logwarn("only " + str(wordcounter) + " arguments provided for waypoint: " + line)
            if goodpoint:
                waypoints.append(line)
    rospy.loginfo("successfully parsed " + str(len(waypoints)) + " waypoints")
    return waypoints

def waypoint_client(wp):
    client = actionlib.SimpleActionClient('fcu/waypoint', asctec_hl_comm.msg.WaypointAction)
    client.wait_for_server()
    goal = asctec_hl_comm.msg.WaypointGoal()

    goal.goal_pos.x = 0
    goal.goal_pos.y = 0
    goal.goal_pos.z = 0
    goal.goal_yaw = 0
    goal.max_speed.x = 0
    goal.max_speed.y = 0
    goal.max_speed.z = 0
    goal.accuracy_position = 0
    goal.accuracy_orientation = 0
    goal.timeout = 0
    try:
        goal.goal_pos.x = float(wp.split()[0])
        goal.goal_pos.y = float(wp.split()[1])
        goal.goal_pos.z = float(wp.split()[2])
        goal.goal_yaw = float(wp.split()[3])
        goal.max_speed.x = float(wp.split()[4])
        goal.max_speed.y = float(wp.split()[5])
        goal.max_speed.z = float(wp.split()[6])
        goal.accuracy_position = float(wp.split()[7])
        goal.accuracy_orientation = float(wp.split()[8])
        goal.timeout = float(wp.split()[9])
    except: 
        pass
    
    rospy.loginfo("sending waypoint: " + "\t" + str(goal.goal_pos.x) + "\t" + str(goal.goal_pos.y) + "\t" + str(goal.goal_pos.z))
    client.send_goal(goal, done_cb, active_cb, feedback_cb)
    if client.wait_for_result():    # Waits for the server to finish performing the action.
        try:
            time.sleep(float(wp.split()[10]))   # only remain at the reached position if goal successfully reached
        except:
            pass

def active_cb():
    rospy.loginfo("waypoint activated")

def done_cb(stat, result):
    rospy.loginfo("waypoint reached at: " + "\t" + str(result.result_pos.x) + "\t" + str(result.result_pos.y) + "\t" + str(result.result_pos.z))

def feedback_cb(feedback):
    rospy.loginfo("MAV currently at: " + "\t" + str(feedback.current_pos.x) + "\t" + str(feedback.current_pos.y) + "\t" + str(feedback.current_pos.z))

if __name__ == '__main__':
    try:
        # Initializes a rospy node so that the SimpleActionClient can
        # publish and subscribe over ROS.
        rospy.init_node('wpclient')
        liste = parseWaypoints();
        for line in liste:
            waypoint_client(line)
    except rospy.ROSInterruptException:
        print "program interrupted before completion"


